`timescale 1ns / 1ps
//////////////////////////////////////////////////////////////////////////////////
// Company: 
// Engineer: 
// 
// Create Date: 2024/08/12 21:55:05
// Design Name: 
// Module Name: motor_top
// Project Name: 
// Target Devices: 
// Tool Versions: 
// Description: 
// 
// Dependencies: 
// 
// Revision:
// Revision 0.01 - File Created
// Additional Comments:
// 
//////////////////////////////////////////////////////////////////////////////////

`include "foc_header.vh"
module motor_top(   
    input                   sys_clk_i                   ,
    input                   sys_rst_n_i                 ,

    //外部配置参数输入
    input[31:0]             foc_mode_i                  ,
    input[31:0]             foc_calibration_en_i        ,
    input signed[31:0]      foc_open_uq_i               ,
    input signed[31:0]      foc_open_ud_i               ,
    input signed[31:0]      foc_Iq_aim_i                ,
    input signed[31:0]      foc_Iq_pid_p_i              ,
    input signed[31:0]      foc_Iq_pid_i_i              ,
    input signed[31:0]      foc_Id_aim_i                ,
    input signed[31:0]      foc_Id_pid_p_i              ,
    input signed[31:0]      foc_Id_pid_i_i              ,
    input signed[31:0]      foc_speed_aim_i             ,
    input signed[31:0]      foc_speed_pid_p_i           ,
    input signed[31:0]      foc_speed_pid_i_i           ,
    input signed[31:0]      foc_position_aim_i          ,
    input signed[31:0]      foc_position_pid_p_i        ,
    input signed[31:0]      foc_position_pid_i_i        ,

    input[7:0]              wavedata_sel_i              ,
    //uart
    output                  uart_tx_pin_o               ,

    //adc pin
    output                  ad7928_cs_pin_o             ,
    output                  ad7928_sclk_pin_o           ,
    output                  ad7928_mosi_pin_o           ,
    input                   ad7928_miso_pin_i           ,
    //as5047p spi 接口
    output                  as5047p_spi_ss_pin_o        ,     
    output                  as5047p_spi_sck_pin_o       ,    
    output                  as5047p_spi_mosi_pin_o      ,   
    input                   as5047p_spi_miso_pin_i      ,
    //as5047p ABZ 接口
    input                   as5047p_A_pin_i             ,
    input                   as5047p_B_pin_i             ,
    input                   as5047p_I_pin_i             ,

    //电机UVW引脚输出
    output                  svpwm_en_o                  ,
    output                  svpwm_u_pin_o               ,
    output                  svpwm_v_pin_o               ,
    output                  svpwm_w_pin_o               
);
assign svpwm_en_o           =  1'b1;




localparam  S_POWER_DELAY                    =   'd0;
localparam  S_ADC_CALIBRATION                =   'd1;
localparam  S_STOP                           =   'd2;        //暂停
localparam  S_CALIBRATION                    =   'd3;        //校准电机 零偏和极对数
localparam  S_WORK                           =   'd4;        //运行
localparam  S_SELFTEST                       =   'd5;        //自测模式

reg[5:0]            state , next_state              ;


wire                pwr_delay_ack                   ;
reg[19:0]           pwr_delay_cnt                   ;

wire                sleftest_en                     ;
wire[5:0]           sleftest_foc_mode               ;
wire[31:0]          selftest_angle                  ;
wire signed[31:0]   selftest_Uq                     ;     //自测模式下Uq参数
wire signed[31:0]   selftest_Ud                     ;     //自测模式下Ud参数



wire [31:0]         motor_electrical_angle          ;     //经过零偏校准后的角度

wire                calibration_en                  ;
wire                calibration_ack                 ;
wire                calibration_encoder_dir         ;
wire [31:0]         calibration_angle_offset        ;
wire [7:0]          calibration_motor_pole_pair     ;
wire [5:0]          calibration_foc_mode            ;
wire [31:0]         calibration_Open_angle          ;
wire signed[31:0]   calibration_Open_Uq             ;
wire signed[31:0]   calibration_Open_Ud             ;



reg[5:0]                foc_mode                    ;
reg[31:0]               foc_angle                   ;
reg signed[31:0]        Open_Uq                     ;
reg signed[31:0]        Open_Ud                     ;

wire                    senser_tirgger              ;


wire                    adc_offset_detect_req       ;
wire                    adc_convert_req             ;
wire                    adc_offset_detect_ack       ;
wire                    adc_convert_ack             ;
wire signed[31:0]       Ia                          ;
wire signed[31:0]       Ib                          ;
wire signed[31:0]       Ic                          ;
wire[15:0]              power_voltage               ;

wire                    motor_position_en           ; //电机位置检测使能
wire                    motor_position_set          ; //设置电机当前位置为初始位置        
wire [31:0]             motor_angle                 ; //电机当前角度
wire signed[31:0]       motor_speed                 ; //电机当前速度
wire signed[31:0]       motor_position              ; //电机当前位置
wire                    motor_one_circle            ; //电机运行一圈标志输出
wire [15:0]             abz_one_circle_plus         ; //电机 ABZ接口一圈的脉冲数

wire signed[31:0]       foc_Iq_o                    ;
wire signed[31:0]       foc_Id_o                    ;


//串口协议数据
wire[7:0]               func2_o                     ;
wire signed[31:0]       data1_o                     ;
wire signed[31:0]       data2_o                     ;
wire signed[31:0]       data3_o                     ;
wire signed[31:0]       data4_o                     ;


assign motor_position_en = (  foc_mode_i[5:0] == `FOC_MODE_POSTION ) ? 1'b1 : 1'b0;
assign motor_position_set = 1'b0;

assign pwr_delay_ack = ( pwr_delay_cnt == 'd50_0000 ) ? 1'b1 : 1'b0;

assign adc_offset_detect_req = ( state == S_ADC_CALIBRATION ) ? 1'b1 : 1'b0;
assign adc_convert_req       = senser_tirgger;

assign calibration_en = ( state == S_CALIBRATION ) ? 1'b1 : 1'b0;
assign sleftest_en    = ( state == S_SELFTEST ) ? 1'b1 : 1'b0;


always@( posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        state <= S_ADC_CALIBRATION;
    else
        state <= next_state;
end


always@(*) begin
    case( state )
    S_POWER_DELAY:
        if( pwr_delay_ack == 1'b1 )
            next_state <= S_ADC_CALIBRATION;
        else
            next_state <= S_POWER_DELAY;
    S_ADC_CALIBRATION:
        if( adc_offset_detect_ack == 1'b1 )
            next_state <= S_STOP;
        else
            next_state <= S_ADC_CALIBRATION;
    S_STOP:
        if( foc_calibration_en_i[0] == 1'b1 )
            next_state <= S_CALIBRATION;
        else if( foc_mode_i[5:0] == `FOC_MODE_SELFTEST )
            next_state <= S_SELFTEST;
        else if( foc_mode_i[5:0] == `FOC_MODE_OPEN || foc_mode_i[5:0] == `FOC_MODE_TORQUE )
            next_state <= S_WORK;
        else if( foc_mode_i[5:0] == `FOC_MODE_SPEED || foc_mode_i[5:0] == `FOC_MODE_POSTION )
            next_state <= S_WORK;
        else
            next_state <= S_STOP;
    S_CALIBRATION:
        if( calibration_ack == 1'b1 )
            next_state <= S_STOP;
        else
            next_state <= S_CALIBRATION; 
    S_SELFTEST:
        if( foc_mode_i[5:0] == `FOC_MODE_STOP )
            next_state <= S_STOP;
        else
            next_state <= S_SELFTEST;
    S_WORK:
        if( foc_mode_i[5:0] == `FOC_MODE_STOP )
            next_state <= S_STOP;
        else
            next_state <= S_WORK;
    default: next_state <= S_ADC_CALIBRATION;
    endcase
end


always@(posedge sys_clk_i or negedge sys_rst_n_i ) begin
    if( sys_rst_n_i == 1'b0 )
        pwr_delay_cnt <= 'd0;
    else if( state == S_POWER_DELAY )
        pwr_delay_cnt <=  pwr_delay_cnt + 1'b1;
    else
        pwr_delay_cnt <= pwr_delay_cnt;

end

always@( posedge sys_clk_i  ) begin
    if( state == S_CALIBRATION ) begin
        foc_mode <= calibration_foc_mode;
        Open_Uq  <= calibration_Open_Uq;
        Open_Ud  <= calibration_Open_Ud;
    end
    else if( state == S_SELFTEST ) begin
        foc_mode <= sleftest_foc_mode;
        Open_Uq  <= selftest_Uq;
        Open_Ud  <= selftest_Ud;
    end
    else begin
        foc_mode <= foc_mode_i[5:0];
        Open_Uq  <= foc_open_uq_i;
        Open_Ud  <= foc_open_ud_i;
    end
end

always@( posedge sys_clk_i ) begin
    if( state == S_CALIBRATION )
        foc_angle <= calibration_Open_angle;
    else if( state == S_SELFTEST )
        foc_angle <= selftest_angle;
    else
        foc_angle <= motor_electrical_angle;
end

       

//自测模块--用于检测电机硬件是否OK
motor_selftest motor_selftest_hp(
    .sys_clk_i                  (   sys_clk_i                   ),
    .sys_rst_n_i                (   sys_rst_n_i                 ),

    .sleftest_en_i              (   sleftest_en                 ),

    .foc_mode_o                 (   sleftest_foc_mode           ),
    .selftest_angle_o           (   selftest_angle              ),
    .selftest_Uq_o              (   selftest_Uq                 ),     //自测模式下Uq参数
    .selftest_Ud_o              (   selftest_Ud                 )      //自测模式下Ud参数
);

//将机械角度转为电角度 并归一化
mechanical2electrical_angle mechanical2electrical_angle_hp(
    .sys_clk_i                  (   sys_clk_i                   ),
    .sys_rst_n_i                (   sys_rst_n_i                 ),
    .encoder_dir_i              (   calibration_encoder_dir     ),
    .encoder_angle_i            (   motor_angle                 ),
    .encoder_angle_offset_i     (   calibration_angle_offset    ),
    .motor_pole_pair_i          (   calibration_motor_pole_pair ),

    .motor_angle_o              (   motor_electrical_angle      )     
);

//校准模块
motor_calibration motor_calibration_hp(
    .sys_clk_i                  (   sys_clk_i                   ),
    .sys_rst_n_i                (   sys_rst_n_i                 ),

    .calibration_en_i           (   calibration_en              ),
    .calibration_ack_o          (   calibration_ack             ),

    .encoder_one_circle_i       (   motor_one_circle            ),
    .encoder_angle_i            (   motor_angle                 ),

    .encoder_dir_o              (   calibration_encoder_dir     ),
    .encoder_angle_offset_o     (   calibration_angle_offset    ),
    .motor_pole_pair_o          (   calibration_motor_pole_pair ),

    .foc_mode_o                 (   calibration_foc_mode        ),
    .Open_angle_o               (   calibration_Open_angle      ),
    .Open_Uq_o                  (   calibration_Open_Uq         ),     //开环模式下Uq参数
    .Open_Ud_o                  (   calibration_Open_Ud         )      //开环模式下Ud参数
);

//FOC算法模块
FOC FOC_hp(
    .sys_clk_i                  (   sys_clk_i           ),
    .sys_rst_n_i                (   sys_rst_n_i         ),

    .foc_mode_i                 (   foc_mode            ),       //FOC运行模式
    .Open_Uq_i                  (   Open_Uq             ),       //开环模式下Uq参数输入
    .Open_Ud_i                  (   Open_Ud             ),       //开环模式下Ud参数输入

    .desired_position_i         (   foc_position_aim_i  ),
    .desired_speed_i            (   foc_speed_aim_i     ),
    .desired_Iq_i               (   foc_Iq_aim_i        ),
    .desired_Id_i               (   foc_Id_aim_i        ),
    //三环控制参数
    .Torque_Iq_PID_P_i          (   foc_Iq_pid_p_i      ),
    .Torque_Iq_PID_I_i          (   foc_Iq_pid_i_i      ),
    .Torque_Id_PID_P_i          (   foc_Id_pid_p_i      ),
    .Torque_Id_PID_I_i          (   foc_Id_pid_i_i      ),
    .Speed_PID_P_i              (   foc_speed_pid_p_i   ),
    .Speed_PID_I_i              (   foc_speed_pid_i_i   ),
    .Position_PID_P_i           (   foc_position_pid_p_i),
    .Position_PID_I_i           (   foc_position_pid_i_i),
    //电机参数输出
    .foc_Ialpha_o               (   foc_Ialpha_o        ),
    .foc_Ibeta_o                (   foc_Ibeta_o         ),
    .foc_Iq_o                   (   foc_Iq_o            ),
    .foc_Id_o                   (   foc_Id_o            ),
    .foc_time_u_o               (   foc_time_u_o        ),
    .foc_time_v_o               (   foc_time_v_o        ),
    .foc_time_w_o               (   foc_time_w_o        ),
    //外部传感器数据输入
    .foc_angle_i                (   foc_angle           ),
    .foc_speed_i                (   motor_speed         ),
    .foc_position_i             (   motor_position      ),
    .Ia_i                       (   Ia                  ),
    .Ib_i                       (   Ib                  ),
    .Ic_i                       (   Ic                  ),
    .senser_tirgger_o           (   senser_tirgger      ),   //传感器触发信号
    .senser_ack_o               (   adc_convert_ack     ),   //传感器转换完成
    //无刷电机控制管脚
    .svpwm_u_pin_o              (   svpwm_u_pin_o       ),
    .svpwm_v_pin_o              (   svpwm_v_pin_o       ),
    .svpwm_w_pin_o              (   svpwm_w_pin_o       )
);



//ADC采样模块，获取三相电流和电源电压
adc_top adc_top_hp(
    .sys_clk_i                  (   sys_clk_i                   ),
    .sys_rst_n_i                (   sys_rst_n_i                 ),

    .adc_offset_detect_req_i    (   adc_offset_detect_req       ),
    .adc_convert_req_i          (   adc_convert_req             ),
    .adc_offset_detect_ack_o    (   adc_offset_detect_ack       ),
    .adc_convert_ack_o          (   adc_convert_ack             ),

    .Ia_o                       (   Ia                          ),
    .Ib_o                       (   Ib                          ),
    .Ic_o                       (   Ic                          ),
    .voltage_o                  (   power_voltage               ),
    //adc pin
    .ad7928_cs_pin_o            (   ad7928_cs_pin_o             ),
    .ad7928_sclk_pin_o          (   ad7928_sclk_pin_o           ),
    .ad7928_mosi_pin_o          (   ad7928_mosi_pin_o           ),
    .ad7928_miso_pin_i          (   ad7928_miso_pin_i           ) 
);

//as5047磁编码器
AS5047P_Top AS5047P_Top_hp(
    .sys_clk_i                   (      sys_clk_i                   ),
    .sys_rst_n_i                 (      sys_rst_n_i                 ),

    //角度 位置 速度 信息输出
    .motor_position_en_i         (      motor_position_en           ), //电机位置检测使能
    .motor_position_set_i        (      motor_position_set          ), //设置电机当前位置为初始位置        
    .motor_angle_o               (      motor_angle                 ), //电机当前角度
    .motor_speed_o               (      motor_speed                 ), //电机当前速度
    .motor_position_o            (      motor_position              ), //电机当前位置
    .motor_one_circle_o          (      motor_one_circle            ), //电机运行一圈标志输出
    .abz_one_circle_plus_o       (      abz_one_circle_plus         ), //电机 ABZ接口一圈的脉冲数
    //spi 接口
    .as5047p_spi_csn_p_o         (      as5047p_spi_ss_pin_o        ),
    .as5047p_spi_sclk_p_o        (      as5047p_spi_sck_pin_o       ),   // max 10Mhz
    .as5047p_spi_miso_p_i        (      as5047p_spi_miso_pin_i      ),
    .as5047p_spi_mosi_p_o        (      as5047p_spi_mosi_pin_o      ),
    //ABZ 接口
    .as5047p_A_p_i               (      as5047p_A_pin_i             ),  
    .as5047p_B_p_i               (      as5047p_B_pin_i             ),
    .as5047p_I_p_i               (      as5047p_I_pin_i             )
);

//根据wavedata_sel_i 选择对应的数据通过串口协议上报给上位机进行显示
uart_tx_wavedata_sel uart_tx_wavedata_sel_hp(
    .sys_clk_i                  (      sys_clk_i                    ),
    .sys_rst_n_i                (      sys_rst_n_i                  ),

    .wavedata_sel_i             (   wavedata_sel_i                  ),

    .Ia_i                       (   Ia                              ),
    .Ib_i                       (   Ib                              ),
    .Ic_i                       (   Ic                              ),

    .foc_Iq_i                   (   foc_Iq_o                        ),
    .foc_Id_i                   (   foc_Id_o                        ),
    .desired_Iq_i               (   foc_Iq_aim_i                    ),
    .desired_Id_i               (   foc_Id_aim_i                    ),


    .motor_speed_i              (   motor_speed                     ),
    .desired_speed_i            (   foc_speed_aim_i                 ),
    .foc_angle                  (   foc_angle                       ),


    .motor_position_i           (   motor_position                  ),
    .desired_position_i         (   foc_position_aim_i              ),



    .func2_o                    (   func2_o                         ),
    .data1_o                    (   data1_o                         ),
    .data2_o                    (   data2_o                         ),
    .data3_o                    (   data3_o                         ),
    .data4_o                    (   data4_o                         ) 
);
//串口协议
uart_tx_package uart_tx_package_hp(
    .sys_clk_i                  (      sys_clk_i                    ),
    .sys_rst_n_i                (      sys_rst_n_i                  ),

    .package_tx_req             (       senser_tirgger              ),
    .package_tx_ack             (                                   ),

    .package_head               (       8'hDE                       ),
    .package_func1              (       8'h1A                       ),
    .package_func2              (       func2_o                     ),
    .package_func3              (       8'h00                       ),
    .package_data1              (       data1_o                     ),
    .package_data2              (       data2_o                     ),
    .package_data3              (       data3_o                     ),
    .package_data4              (       data4_o                     ),
    .package_motor_id           (       'd2                         ),
    .package_id                 (       'd2                         ),
    .package_tail               (       8'hED                       ),

    .uart_tx_pin_o              (       uart_tx_pin_o               )
);




endmodule
